/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package org.usfirst.frc3950.Robot2013.commands;
import edu.wpi.first.wpilibj.command.Command;
import org.usfirst.frc3950.Robot2013.Robot;

/**
 *
 * @author DriverStation
 */
public class FixedShooterAngleCommand extends Command {
    private final double SHOOTING_ANGLE = 43.0;//20.0;
    private final double SHOOTING_DELAY_SECS = 2.0;
    private double angleVoltage = 0.0;
    
    public FixedShooterAngleCommand() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
	
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
        requires(Robot.shootingScrew);
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
        this.setTimeout(SHOOTING_DELAY_SECS);
    
    }
    // Called just before this Command runs the first time
    protected void initialize() {
    }
    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
        Robot.shootingScrew.setPIDSubsystem(true);
        angleVoltage = Robot.shootingScrew.setInclineAngle(SHOOTING_ANGLE, false);
    }
    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        if (Robot.shootingScrew.hasReachedTargetVoltage(angleVoltage) && this.isTimedOut())
        {
            return true;
        }
        
        return false;
    }
    // Called once after isFinished returns true
    protected void end() {
    }
    
    public boolean isInterruptible() {
        return true;
    }
    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
        Robot.shootingScrew.setPIDSubsystem(false);
    }
    
}
